#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTServo,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     rightFront,    tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rightBack,     tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     leftBack,      tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     leftFront,     tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     beaterBar,     tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C2_2,     lift,          tmotorTetrix, openLoop, encoder)
#pragma config(Servo,  srvo_S2_C1_1,    backRightServo,       tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    backLeftServoxvxv,    tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    topServo,             tServoStandard)
#pragma config(Servo,  srvo_S2_C1_4,    backLeftServo,        tServoStandard)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//all naming is this program are based of positive y being forwards and positive x being right
#include "JoystickDriver.c"

#include "../drivers-3x/hitechnic-sensormux.h"
#include "../drivers-3x/hitechnic-irseeker-v2.h"
#include "../drivers-3x/hitechnic-gyro.h"
#include "../drivers-3x/hitechnic-eopd.h"
#include "../drivers-3x/lego-light.h"

const tMUXSensor IRS1 = msensor_S4_1;
const tMUXSensor IRS2 = msensor_S4_2;
const tMUXSensor HTEOPD = msensor_S4_3;
const tMUXSensor LEGOLS = msensor_S4_4;

short speedMod = 2;
short straifMod = 1;
short turnMod = 1;
float backMod = 6;
float frontMod = 1;
short zero = 20;
float liftmod = 75;

float high = -11.25*360*4;
float mid = -8*360*4;
float low = -4.5*360*4;

void initializeRobot(){
	nMotorEncoder[lift] = 0;
	servo[topServo] = 235;
	servo[backRightServo] = 50;
	servo[backLeftServo] = 256;
	wait1Msec(500);
	servo[backLeftServo] = 127;
  motor[beaterBar] = 50;
  wait1Msec(1000);
  motor[beaterBar] = 0;
}
float xx = 0;
void liftPos(float x){
	servo[topServo] = 235;
	if(x == 1){
		x = low;
	}else if(x == 2){
		x = mid;
	}else if(x == 3){
		x = high;
	}else if(x == 0){
		x = 0;
	}
	xx = x;
	//remember that x is negative bc lift up is counting down and lift down is counting up probs

}
void ySet(short speed){
	speed /= speedMod;
	motor[leftFront] = speed;
	motor[leftBack] = speed;
	motor[rightFront] = speed;
	motor[rightBack] = speed;
}
void xSet(short speed){
	speed /= straifMod;
	motor[leftFront] = -speed/frontMod;
	motor[leftBack] = speed/backMod;
	motor[rightFront] = speed/frontMod;
	motor[rightBack] = -speed/backMod;
}
void turn(short speed){
	speed /= turnMod;
	motor[leftBack] = speed;
	motor[rightBack] = -speed;
	motor[leftFront] = speed;
	motor[rightFront] = -speed;
}
bool liftCheck(float x){
	if(nMotorEncoder[lift] >= 0 + x && nMotorEncoder[lift] <= -(11.25*360) + x){
		return false;
	}
	return true;
}
void servoPos(short x, bool b){
	servo[backLeftServo] = x;
	if(b){
		servo[backRightServo] = 256;
		if(ServoValue[backRightServo] > servoTarget[backRightServo]){
				servo[backRightServo] = 127;
		}
	}else{
		servo[backRightServo] = 0;
		if(ServoValue[backRightServo] < servoTarget[backRightServo]){
				servo[backRightServo] = 127;
		}
	}
}
task main(){
  initializeRobot();
  // wait for start of tele-op phase
  waitForStart();
  //actual control of the robot
  int _raw = 0;
  int _processed = 0;

  // Set the sensor to short range
  //HTEOPDsetShortRange(HTEOPD);
  while (true){
		getJoystickSettings(joystick);
		//code for robot panning controlled by the left joystick
		if(joystick.joy1_y1 > zero){ //forwards
			ySet(joystick.joy1_y1);
		}else if(joystick.joy1_y1 < -zero){ //backwards
			ySet(joystick.joy1_y1);
		}else if(joystick.joy1_x1 < -zero){ //left
			xSet(joystick.joy1_x1);
		}else if(joystick.joy1_x1 > zero){ //right
			xSet(joystick.joy1_x1);
		}else if(joystick.joy1_x2 < -zero){ //left turn
			turn(joystick.joy1_x2);
		}else if(joystick.joy1_x2 > zero){ //right turn
			turn(joystick.joy1_x2);
		}else{
			ySet(0);
		}

		//button actions
		switch(joystick.joy1_Buttons){
			case 1:
				servoPos(50, true);
				break;
			case 2:
				servoPos(190, false);
				break;
			default:
				break;
		}

		if(joystick.joy2_TopHat != -1){
    	if(joystick.joy2_TopHat == 0){
				liftPos(2);
    	}else if(joystick.joy2_TopHat == 1){
    	}else if(joystick.joy2_TopHat == 2){
    		liftPos(3);
    	}else if(joystick.joy2_TopHat == 3){
    	}else if(joystick.joy2_TopHat == 4){
				liftPos(0);
    	}else if(joystick.joy2_TopHat == 6){
    		liftPos(1);
      }else if(joystick.joy2_TopHat == 5){
    	}else if(joystick.joy2_TopHat == 7){
    	}
    }

    /*
    	JOYSTICK_2!!!!!!!!!!!!!!
    */
    if(joystick.joy2_Buttons & 0x04){
    	//servo[topServo] = 235;
    	motor[beaterBar] = 50;
    }else if(joystick.joy2_Buttons & 0x08){
    	//servo[topServo] = 100;
    	motor[beaterBar] = 0;
    }else if(joystick.joy2_Buttons & 0x01){
    	//motor[beaterBar] = 50;
    	servo[topServo] = 235;
    }else if(joystick.joy2_Buttons & 0x02){
    	//motor[beaterBar] = 0;
    	servo[topServo] = 100;
    }else if(joy2Btn(6) == 1){
    	motor[beaterBar] = -50;
    }else if(joy2Btn(9) == 1 && joy2Btn(10) == 1){
   		xx = 42;
    	nMotorEncoder[lift] = 0;
    }

    if(joystick.joy2_y1 > zero){ //up
    	xx = 42;
			motor[lift] = joystick.joy2_y1;
		}else if(joystick.joy2_y1 < -zero){ //down
			xx = 42;
			motor[lift] = joystick.joy2_y1;
		}else{
			motor[lift] = 0;
		}

		if(xx == 42){
	  }else if(nMotorEncoder[lift] >= xx){
				motor[lift] = 100;
				if(nMotorEncoder[lift] <= xx){
					motor[lift] = 0;
					xx = 42;
				}
		}else if(nMotorEncoder[lift] <= xx){
	   		motor[lift] = -100;
	   		if(nMotorEncoder[lift] >= xx){
	   			motor[lift] = 0;
	   			xx = 42;
	   		}
		}
		/*
		// Read the raw sensor value
    _raw = HTEOPDreadRaw(HTEOPD);
    // read the processed value which is linear with
    // the distance detected.  Use the processed value
    // when you want to determine distance to an object
    _processed = HTEOPDreadProcessed(HTEOPD);
    nxtDisplayClearTextLine(3);
    nxtDisplayClearTextLine(4);
    nxtDisplayTextLine(4, "Proc:  %4d", _processed);
    nxtDisplayTextLine(3, "Raw :  %4d", _raw);
    wait1Msec(50);
    if (_processed > 41) {
      LSsetActive(LEGOLS); // turn light on
      PlaySound(soundBeepBeep);
      while(bSoundActive){};
      LSsetInactive(LEGOLS); // turn light off
    } // if ball close
		*/
	}
}
